(load "/Users/garaemon/prog/roll/src/lisp/util/util.lisp")
(load "/Users/garaemon/prog/roll/src/lisp/search/graph.lisp")
(use-package :roll/search/graph)

(defun test1 ()
  (let* ((g (make-instance '<graph>))
         (solver (make-instance '<a*-graph-search-solver> :problem g :heuristic #'(lambda (x p) 0.0))))
    (let ((test1 (make-instance '<node> :name 'test1))
          (test2 (make-instance '<node> :name 'test2))
          (test3 (make-instance '<node> :name 'test3)))
      (add-node g test1)
      (add-node g test2)
      (add-node g test3)
      (add-arc g test1 test2)
      (add-arc g test2 test3)
      (add-arc g test3 test1)
      (setf (roll/search/graph::start-state-of g) (find-node g 'test1))
      (setf (roll/search/graph::goal-state-of g) (find-node g 'test3))
      (let ((path (solve solver g :debug nil)))
        (mapcar #'roll/search/graph::state-of path)
        ))))

(defun test2 ()
  (let* ((graph (make-instance '<graph>))
         (solver (make-instance '<a*-graph-search-solver>
                                :problem graph
                                :heuristic #'(lambda (x p) 0.0))))
    (let ((a (make-instance '<node> :name 'a))
          (b (make-instance '<node> :name 'b))
          (c (make-instance '<node> :name 'c))
          (d (make-instance '<node> :name 'd))
          (e (make-instance '<node> :name 'e))
          (f (make-instance '<node> :name 'f))
          (g (make-instance '<node> :name 'g))
          (h (make-instance '<node> :name 'h)))
      (add-node graph a)
      (add-node graph b)
      (add-node graph c)
      (add-node graph d)
      (add-node graph e)
      (add-node graph f)
      (add-node graph g)
      (add-node graph h)
      (add-arc graph a b :both t)
      (add-arc graph b c :both t)
      (add-arc graph c d :both t)
      (add-arc graph d e :both t)
      (add-arc graph b f :both t)
      (add-arc graph f g :both t)
      (add-arc graph f h :both t)
      (add-arc graph h d :both t)
      (setf (roll/search/graph::start-state-of graph) (find-node graph 'g))
      (setf (roll/search/graph::goal-state-of graph) (find-node graph 'e))
      (let ((path (solve solver graph :debug nil)))
        (mapcar #'roll/search/graph::state-of path))
      )
    ))
